#include "ata.h"

#define BIT0 1
#define BIT1 2
#define BIT2 4
#define BIT3 8
#define BIT4 16
#define BIT5 32
#define BIT6 64
#define BIT7 128
#define BIT8 256
#define BIT9 512
#define BIT10 1024
#define BIT11 2048
#define BIT12 4096
#define BIT13 8192
#define BIT14 16384
#define BIT15 32768

void loadComm(int ata, int rdwr, unsigned short sector);
void readDisk(int ata, char * msg, int bytes, unsigned short sector, int offset);
void readOneSector(int ata, char * msg, int bytes, unsigned short sector, int offset);
unsigned short getDataRegister(int ata);
void writeDisk(int ata, char * msg, int bytes, unsigned short sector, int offset);

void writeOneSector(int ata, char * msg, int bytes, unsigned short sector, int offset);
void writeDataToRegister(int ata, unsigned char upper, unsigned char lower);

/* Lee de disco en "msg" con tamaño "bytes" a partir del "sector" con un "offset".*/
void readDisk(int ata, char * msg, int bytes, unsigned short sector, int offset){
	int i;
	
	if(bytes == 0)
		return;

	// cantidad de sectores a leer
	int sectors = ((bytes-1) / 512) + 1;

	for(i=0;i<sectors;i++){
		int size =  (i == sectors-1) ? bytes%513 : 512;

		if(i==0)	
			readOneSectorCache(ata, msg, (offset+bytes>512)? size-offset : size, sector, offset);
		else 
			readOneSectorCache(ata, msg+(i*512)-offset,size, sector+i,0);
	}
}

/* Lee de disco en "msg" con tamaño "bytes" en el "sector" con un "offset". (un solo sector)*/
void readOneSector(int ata, char * msg, int bytes, unsigned short sector, int offset){

	// si es mayor a un sector sale
	if(bytes > 512 - offset)
		return;

	char tmp[512];
	loadComm(ata, WIN_READ, sector);
	
	int b;
	unsigned short data;
	for(b=0;b<512;b+=2){
		data = getDataRegister(ata);
		(tmp+b)[0] = data & 0xFF;
		(tmp+b)[1] = data >> 8;
	}

	int i;
	for(i=0;i<bytes;i++)
		msg[i] = tmp[offset+i];
}

/* Escribe en disco el "msg" de tamaño "bytes" a partir del "sector" con un "offset".*/
void writeDisk(int ata, char * msg, int bytes, unsigned short sector, int offset){
	int i;

	if(bytes == 0)
		return;

	// cantidad de sectores a escribir
	int sectors = (bytes / 512) + 1;
	
	for(i=0;i<sectors;i++){

		int size =  (i == sectors-1) ? bytes%513 : 512;
		// First sector, check offset
		if(i==0)
			writeOneSectorCache(ata, msg, (offset+bytes > 512 )? size-offset :size, sector,offset);
		else 
			writeOneSectorCache(ata, msg+(i*512)-offset, size, sector+i, 0);
		
	}
}

/* Escribe en disco el "msg" de tamaño "bytes" en el "sector" con un "offset". (un solo sector)*/
void writeOneSector(int ata, char * msg, int bytes, unsigned short sector, int offset){
	int i = 0;
	int b;

	// si es mayor a un sector sale
	if(bytes > 512 - offset)
		return;

	char tmp[512];
	
	readOneSector(ata,tmp,512,sector,0);

	for(i=0;i<bytes;i++)
		tmp[offset+i] = msg[i];

	loadComm(ata, WIN_WRITE, sector);

	for(b=0;b<=512;b+=2)
		writeDataToRegister(ata, tmp[b+1], tmp[b]);
}

void writeDataToRegister(int ata, unsigned char upper, unsigned char lower){
	 
	unsigned short out;
	
	while (!(portw_in(ata + ATA_REG7) & BIT3));
	
	out = (upper << 8) | lower;
	portw_out(ata + ATA_REG0, out);
	
	 
}

unsigned short getDataRegister(int ata){
	 
	unsigned short ans;
	
	while (!(portw_in(ata + ATA_REG7) & BIT3));

	ans = portw_in(ata + ATA_REG0);
	
	 
	return ans;
}

unsigned short getErrorRegister(int ata){
	 
	unsigned short rta = port_in(ata + ATA_REG1) & 0x00000FFFF;
	 
	return rta;
}

void loadComm(int ata, int rdwr, unsigned short sector){
	 
	
	port_out(ata + ATA_REG1, 0);
	port_out(ata + ATA_REG2, 0);
	
	port_out(ata + ATA_REG3, (unsigned char)sector);		
	port_out(ata + ATA_REG4, (unsigned char)(sector >> 8));	
	port_out(ata + ATA_REG5, (unsigned char)(sector >> 16));
	port_out(ata + ATA_REG6, 0xE0 | (ata << 4) | ((sector >> 24) & 0x0F));
	
	
	port_out(ata + ATA_REG7, rdwr);


	 
}

